#include "Bond.h"

static void
normalize (vector<vector<double> > & ala,
	   const vector<double > & box)
{
  // if (ala.size() <= 1){
  //   return ;
  // }
  for (unsigned ii = 1; ii < ala.size() ; ++ii){
    if      (ala[ii][0] - ala[0][0] > 0.5 * box[0]) ala[ii][0] -= box[0];
    else if (ala[ii][0] - ala[0][0] <-0.5 * box[0]) ala[ii][0] += box[0];
    if      (ala[ii][1] - ala[0][1] > 0.5 * box[1]) ala[ii][1] -= box[1];
    else if (ala[ii][1] - ala[0][1] <-0.5 * box[1]) ala[ii][1] += box[1];
    if      (ala[ii][2] - ala[0][2] > 0.5 * box[2]) ala[ii][2] -= box[2];
    else if (ala[ii][2] - ala[0][2] <-0.5 * box[2]) ala[ii][2] += box[2];
  }
}

static vector<double>
crossProd (const vector<double> & diff0,
	   const vector<double> & diff1)
{
  vector<double> re (3);
  re[0] = (diff0[1] * diff1[2] - diff0[2] * diff1[1]);
  re[1] =-(diff0[0] * diff1[2] - diff0[2] * diff1[0]);
  re[2] = (diff0[0] * diff1[1] - diff0[1] * diff1[0]);  
  return re;
}

static double
dotProd (const vector<double> & diff0,
	 const vector<double> & diff1)
{
  return (diff0[0] * diff1[0] +
	  diff0[1] * diff1[1] +
	  diff0[2] * diff1[2] );
}

static vector<double>
calNormVec (vector<vector<double> > & group)
{
  vector<double> diff0 (3), diff1(3);

  for (unsigned dd = 0; dd < 3; ++dd){
    diff0[dd] = group[0][dd] - group[1][dd];
    diff1[dd] = group[2][dd] - group[1][dd];
  }

  return crossProd (diff0, diff1);
}

BondCalculator::
BondCalculator (const vector<double > & box_)
    : box (box_)
{
}


double BondCalculator::
calBond (const vector<vector<double> > & xx)
{
  vector<vector<double> > tmpxx(xx);
  normalize (tmpxx, box);

  if (xx.size() < 2){
    cerr << "size of vector is smaller than 2" << endl;
    exit (1);
  }
  vector<double > dr(3);
  for (unsigned dd = 0; dd < 3; ++dd){
    dr[dd] = tmpxx[1][dd] - tmpxx[0][dd];
  }

  return sqrt (dotProd(dr, dr));
}


void BondCalculator::
calGrad (const vector<vector<double> > & xx,
	 const double & dx,
	 vector<vector<double> > & grad)
{
  for (unsigned ii = 0; ii < 2; ++ ii){
    vector<double > tmpgrad (3);
    for (unsigned dd = 0; dd < 3; ++dd){
      vector<vector<double > > xx0 (xx);
      vector<vector<double > > xx1 (xx);
      xx0[ii][dd] += dx;
      xx1[ii][dd] -= dx;
      double phi0 = calBond (xx0);
      double phi1 = calBond (xx1);
      tmpgrad[dd] = (phi1 - phi0) / (2. * dx);
    }
    grad.push_back (tmpgrad);
  }  
}


double BondCalculator::
calLaplacian (const vector<vector<double> > & xx,
	      const double & dx)
{
  double sum = 0.;
  double phi = calBond (xx);
  for (unsigned ii = 0; ii < 2; ++ii){
    for (unsigned dd = 0; dd < 3; ++dd){
      vector<vector<double > > xx0 (xx);
      vector<vector<double > > xx1 (xx);
      xx0[ii][dd] += dx;
      xx1[ii][dd] -= dx;
      double phi0 = calBond (xx0);
      double phi1 = calBond (xx1);
      sum += (phi1 - 2. * phi + phi0) / (dx * dx);
    }
  }
  return sum;
}
